#ifdef GUI
#include "simulator.h"
#include <time.h>

Simulator *hack;

Mat scan(int i)
{
	Mat y;
	if(hack->map.size() > i) {
		double r_x = hack->r_x;
		double r_y = hack->r_y;
		double r_theta = hack->r_theta;

		double mx=hack->map[i].x, my=hack->map[i].y;
		double mag_rm = sqrt(pow(my-r_y,2)+pow(mx-r_x,2));
		double theta = atan2((my-r_y),(mx-r_x))-r_theta;
		y.push_back(mag_rm);
		y.push_back(theta);
	}

	return y;
}

Simulator::Simulator(Mat q, Mat s)
{
	hack = this;
	disp = new MapDisplay("Map");
	slam = new EkfSlam(scan, q, s);
	//generate map random markers limited to a circle
	time_t t;
	CvRNG rng = cvRNG(time(&t));

	for(int i=0; i < 100; i++) {
		double x = cvRandReal(&rng)*8-4;
		double y = cvRandReal(&rng)*8-4;

		feature f = {x,y};
		map.insert(map.end(), f);

		disp->addFeature(x,y);
	}

	r_x = 0;
	r_y = 0;
	r_theta = 0;
	r_fov = M_PI/3;

	disp->renderMap();
}

void Simulator::step(Mat u)
{
	double r_dp = u.at<double>(0);
	double r_dth = u.at<double>(1);

	Mat rmap, mcov;
	r_x += r_dp*cos(r_theta);
	r_y += r_dp*sin(r_theta);
	r_theta += r_dth;
	slam->step(u);
	disp->moveRobot(r_dp, r_dth);
	if(slam->getMap(rmap, mcov)) {
		for(int i=0; i < rmap.rows; i+=2) {
			double g_x = rmap.at<double>(i);
			double g_y = rmap.at<double>(i+1);

			disp->setGuess(g_x,g_y,i/2);
		}
	}

	Mat robot = slam->getRobot();
	disp->guessRobot(robot.at<double>(0), robot.at<double>(1), robot.at<double>(2));
	disp->renderMap();
}

Simulator::~Simulator()
{
	delete disp;
	delete slam;
}

#endif
